FUNCTION_BLOCK DCO_WriteDriveParameter
TITLE = 'DCO_WriteDriveParameter'

// This FB write the parameters of a connected drive
// The implementation is independant of the used Bussystem
// 
// Versionshistory:
// 1.0 Ersterstellung E.Dittmann 
// 1.1 Timeout ED 
// 2.0 Release

// USED S7 Systemfuctions:
// SFB52 RDREC
// SFB53 WRREC
// SFB4  TON

VERSION : '2.0'
AUTHOR  : 'Lenze'
FAMILY  : 'DCO'
NAME : 'WritePar' // ( 8 Zeichen max.)
KNOW_HOW_PROTECT

VAR_INPUT
    iDiagnosticADR      : INT   := 2000;             // Diagnostic adress of the drive (Hardwareconfig S7)
    xExecute            : BOOL  := FALSE;
    xUseIndexAsParam    : BOOL  := True;            // True = Lenze index number at wIndex            
    iIndex              : INT   := 0;                
    iSubIndex           : INT   := 0;
    iType               : INT   := 7;               // 2=Int8, 3=Int16, 4=Int32, 5=Unsigned8, 6=Unsigned16, 7=Unsigned32, 65=Byte, 66=Word, 67=DWord
    iScalingFactor      : INT   := 1;               // Scaling factor from attributlist
    rValue              : REAL  := 1;
END_VAR

VAR_OUTPUT
    xDone               :BOOL   := False;
    xBusy               :BOOL   := False;
    xError              :BOOL   := False;
    xTimeout            :BOOL   := False;
//    iDataType           :INT    := 0;
    wErrorCode          :WORD   := 0;               // Errorcode 
    wErrorInfo          :Word   := 0;               // Errorcode additional information      
END_VAR

VAR
    xExecute_Old        : BOOL  :=False;
    xReadOld            : BOOL  :=False;
    xWriteReq           : BOOL  :=False;
    xReadReq            : BOOL  :=False;
    wDiagnosticADR      : WORD;
    parWrite            : WRREC;
    xWriteDone          : BOOL;
    xWriteBusy          : BOOL;
    xWriteError         : BOOL;
    dwWriteStatus       : DWORD;
    scWriteRecord       : STRUCT
                            Request_Reference001    : BYTE := B#16#0;  
                            Request_ID_001          : BYTE := B#16#2;  
                            Axis001                 : BYTE := B#16#0;   
                            Number_Parameter001     : BYTE := B#16#1;  
                            Attribute001            : BYTE := B#16#10; 
                            Number_Subcodes001      : BYTE := B#16#1;  
                            Index001                : INT  := 24564;   
                            Subindex001             : INT  := 0;        
                            Format001               : BYTE := 43;        
                            Number_Values001        : BYTE := B#16#1;  
                            ParameterValue001       : DWORD := 0;         
                          END_STRUCT;
    parRead             : RDREC;
    xReadValid          : BOOL;
    xReadBusy           : BOOL;
    xReadError          : BOOL;
    dwReadStatus        : DWord;
    scReadRecord        : STRUCT
                            Request_Refernce011     : BYTE := B#16#0;   
                            Response_ID_011         : BYTE := B#16#0;   
                            Axis011                 : BYTE := B#16#0;   
                            Number_Paramater011     : BYTE := B#16#0;   
                            Format011               : BYTE := B#16#0;   
                            Number_Values011        : BYTE := B#16#0;   
                            Errorcode011            : WORD := 0;        
                            ErrorInfo011            : WORD := 0;        
                          END_STRUCT;
    dwtest1             :DWORD;
    dwtest2             :DWORD;                    
    dwtest3             :REAL; 
    delay               :TON;                
    timeout             :TON;
END_VAR

LABEL
    ENDE;
END_LABEL

// Timeout           
Timeout(IN := xExecute  AND NOT xDone        
          ,PT := T#10s                      
          ); 
// xError:= Timeout.q;
xTimeout:= Timeout.q;

IF NOT xExecute AND NOT xWriteReq AND NOT xReadReq THEN
    GOTO ENDE;
ELSIF NOT xExecute AND ( xWriteReq OR xReadReq )THEN // Reset mit laufendem Auftrag
    xWriteReq := False;
    xReadReq  := False;
END_IF;



wDiagnosticADR:=INT_TO_WORD(iDiagnosticADR);

IF (xExecute = true) AND (xExecute_Old = False) THEN
    xWriteReq := True;
    xExecute_Old := True;
    scReadRecord.ErrorCode011:=0;
    scReadRecord.ErrorInfo011:=0;
    scReadRecord.Response_ID_011:=0;
    scReadRecord.Format011:=0;
    scReadRecord.Number_Paramater011:=0;
    dwtest1:=0;
    dwtest2:=0;
    dwtest3:=0;
    xReadReq  := False;
//    iDataType := 0;

END_IF;

IF xUseIndexAsParam THEN
    scWriteRecord.Index001:= 24575 - iIndex;
ELSE
    scWriteRecord.Index001:= iIndex;
END_IF;

scWriteRecord.Subindex001:=iSubIndex;
scWriteRecord.Format001:=INT_TO_BYTE(iType);
scWriteRecord.Number_Subcodes001:=1; 


CASE iType OF
        02,5,65 :
            scWriteRecord.ParameterValue001:=ROL(IN:=DINT_TO_DWORD(REAL_TO_DINT(rValue * iScalingFactor)), N:=24);
        3,6,33,66 :
            scWriteRecord.ParameterValue001:=ROL(IN:=DINT_TO_DWORD(REAL_TO_DINT(rValue * iScalingFactor)), N:=16);
        4,7,34,67 :
            scWriteRecord.ParameterValue001:=DINT_TO_DWORD(REAL_TO_DINT(rValue * iScalingFactor));
    ELSE:
            scWriteRecord.ParameterValue001:=DINT_TO_DWORD(REAL_TO_DINT(rValue * iScalingFactor));
    END_CASE;


parWrite(REQ := xWriteReq                       // IN: BOOL
            ,ID := wDiagnosticADR 
            ,INDEX := 47                        // immer47
            ,LEN := 16                          // IN: INT
            ,RECORD := scWriteRecord            // INOUT: ANY
            ); 
   xWriteDone := parWrite.DONE;                 // OUT: BOOL
   xWriteBusy := parWrite.BUSY;                 // OUT: BOOL
   xWriteError:= parWrite.ERROR;                // OUT: BOOL
   dwWriteStatus:= parWrite.STATUS;             // OUT: DWORD

  
parRead(REQ := xReadReq                         // IN: BOOL
            ,ID := wDiagnosticADR               // IN: DWORD
            ,INDEX := 47                        // immer 47
            ,MLEN := 10                         // IN: INT
            ,RECORD := scReadRecord             // INOUT: ANY
            ); 
   xReadValid:= parRead.VALID;                  // OUT: BOOL
   xReadBusy:= parRead.BUSY;                    // OUT: BOOL
   xReadError:= parRead.ERROR;                  // OUT: BOOL
   dwReadStatus:= parRead.STATUS;               // OUT: DWORD

xBusy := xWriteBusy OR xReadBusy;

// Verzögerung der Fehlermeldung aufgrund der Statusmeldung "Teilnehmer nicht bereit"
delay(IN :=  xWriteError OR  xReadError         // IN: BOOL
          ,PT := T#1s                           // IN: TIME
          ); 
 

IF  xWriteDone AND NOT xDone AND NOT xReadOld THEN
    xWriteReq := False;
    xReadReq  := True;
    xReadOld  := True;
END_IF;


IF xWriteError AND delay.q THEN
    xError:=True;
    xWriteReq := False;
ELSIF  xReadError AND delay.q THEN
    xError:=True;
    xReadReq:=False;
END_IF;


IF xReadValid  THEN
    xReadReq:=False;
//    iDataType:=BYTE_TO_INT(scReadRecord.Format011);
    wErrorCode:=scReadRecord.ErrorCode011;
    wErrorInfo:=scReadRecord.ErrorInfo011;
    xDone:=True;
    IF (wErrorCode <> 0) OR ( wErrorInfo <> 0)  OR (ScReadRecord.Format011 = 68 ) OR ( scReadRecord.Response_ID_011 = 130 )THEN
        xError:=True;
        dwtest2:= 1;
    END_IF;
    dwtest1:= 1;
END_IF;

ENDE:

IF NOT xExecute THEN
    xExecute_Old:=False;
    xReadOld:=False;
    xError:=False;
    xBusy:=False;  
    xDone:=False;
    xWriteError:=False;
    xReadError:=False;
    xWriteReq := False;
    xReadReq  := False;
END_IF;

END_FUNCTION_BLOCK
